%FDYN2  private function called by FDYN
%
%	XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN)
%
% Called by FDYN to evaluate the robot velocity and acceleration for
% forward dynamics.  T is the current time, X = [Q QD] is the state vector,
% ROBOT is the object being integrated, and TORQUEFUN is the string name of
% the function to compute joint torques and called as
%
%       TAU = TORQUEFUN(T, X)
%
% if not given zero joint torques are assumed.
%
% The result is XDD = [QD QDD].
%
% See also: FDYN

% Copyright (C) 1999-2008, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
% 
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.

function [ts, qs, qds, qdds] = myfdyn(robot, t0, t1, torqfun, q0, qd0, qmin, qmax, varargin)


	n = robot.n;

    if nargin < 5
        q=zeros(1,n);
    else
    	q = q0;    
    end
    if nargin < 6
        qd=zeros(1,n);
    else
    	qd = qd0;    
    end
    if nargin < 7
        qmin=-1.0e6*ones(1,n);
    end
    if nargin < 8
        qmax=1.0e6*ones(1,n);
    end

    
    

    dt = .01;
    ts=t0:dt:t1;
    
    qs=zeros( length(ts), n);
    qds=zeros( length(ts), n);
    qdds=zeros( length(ts), n);
    
    
    i=1;
    for t = ts
        
        % evaluate the torque function if one is given
        if isstr(torqfun)
            tau = feval(torqfun, t, q, qd, varargin{:});
        else
            if isvector(torqfun)
                tau=torqfun;  % it's really a vector of torques.
            else
                tau = zeros(n,1);
            end
        end
        
        qdd = accel(robot, q, qd, tau);
        
        
        old_qd = qd;
        old_q = q;

        % Euler integration        
        qd = qd + dt*qdd';
        q = q + dt*qd;
        
        % clip
        over = find(q > qmax);
        qdd(over) = 0-old_qd(over);
        qd(over)=qmax(over)-old_q(over);
        q(over)=qmax(over);
        
        under = find(q < qmin);
        qdd(under) = 0-old_qd(under);
        qd(under)=qmin(under)-old_q(under);
        q(under)=qmin(under);
       

         
        % store
        qs(i,:)=q;
        qds(i,:)=qd;
        qdds(i,:)=qdd;
        
        
        i=i+1;
    end
    
    
    